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(54) An autopilot device and method for cancelling dynamic operating errors in a system having 
variable parameters 



(57) An autopilot device comprising a regulator (44) 
receiving at its input a plurality of reference signals (R) 
and generating at its output control signals (u(t)) for a 
physical system (40) the dynamic behaviour of which 
can be described by a plurality of state variables (X(t)) 
and a measurement block (42) operable to select, from 
among the state variables (X(t)) ( a subset of measured 
output variables (Y(t)). An estimator (46) modelling the 
physical system (40) receives at its input the control sig- 
nals (u(t)) and generates a plurality of estimated output 
variables (Y{t)) which are compared with the measured 



output variables (Y(t)) to generate an error signal (e(t)) 
utilised by the estimator. In the estimator (46) there is 
provided an extended dynamic system (Z(t)) which has 
as its state variables a plurality of estimated state varia- 
bles (X(0) of the physical system (40) together with a 
plurality of additional state variables which model 
respective imaginary commands (n(0) of unknown 
value which have the same effect on the physical sys- 
tem (40) as the control signals (u(t)) and which intro- 
duce operating errors into the dynamic behaviour of the 
physical system (40) itself. 
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Description 



The present invention relates to an autopilot device and to a method for cancelling operating errors in the dynamic 
behaviour of a system having variable or constant parameters. 
5 With particular reference to Figure 1 , a known type of autopilot device is generally indicated 1 . 

The device 1 is operable to control a physical system 3 which is schematically represented by a block receiving a 
control vector u(t) at its input and generating at its output a plurality of state variables X(t) (defined by a vector). In par- 
ticular, the state variables X(t) are fed to a measurement block 5 which selects from among the various state variables 
X(t) of the physical system 3. some state variables Y(t) (defined by a vector), or a combination of the state variables 
10 which represent the measured state variables (output variables). For example, the physical system 3 may comprise a 
vehicle (not shown) in particular a vehicle which moves in a fluid (for example a submarine) which is provided with a 
plurality of sensors (not shown) which can measure the instantaneous value of the output variables Y(t). 

The device 1 comprises a regulator 7 which receives at its input a plurality of reference signals R (reference set) 
together with a plurality of limits L and generates a said control vector u(t) at its output. 
is The reference signals R are set manually by an operator (not shown) of the autopilot device 1 and represent the 
desired values of the output variables of the physical system 3 controlled by the autopilot device 1 . The device 1 further 
includes an estimator 1 0 which receives at its input the control vector u(t) and the vector Y(t) of the measured variables 
and generates a feedback vector, the nature of which will be clarified hereinafter, fed back by means of a line 12 to the 
regulator 7. The estimator 10 is able to model the dynamic behaviour of the physical system 3 by generating at its output 
20 a vector X{t) comprising the estimated values of the state variables of the system. The estimator 10 is, moreover, able 
to generate at its output a vector Y(t) comprising the estimated values of the measured output variables of the system. 

The regulator 7 determines the control vector u(t) as a function of the reference signals R and the limits set by the 
operator and in dependence on the estimated state variables X(t). 

The output of the measurement block 5 is fed (with positive sign) to a summing block 15 which also receives the 
25 vector Y(t) (estimated output variables) sign inverted; the summing block 15 therefore generates at its output an error 
vector e(t) which represents the difference between the vector Y(t) of the measured output variables and the vector Y(t) 
of the estimated output variables. 

The estimator 10 includes an integrator block 20 which has an input 20a communicating with the output of a sum- 
ming node 22 and an output 20b communicating with a measurement block 25 generating at its output the vector Y(t) 
30 of the estimated output variables. The estimator 1 0 further comprises: 

a first processor block 27 (described in detail hereinafter) having an input 27a connected to the output 20b of the 
integrator block 20 and an output 27b communicating with a summing input of the node 22; 
a second processor block 29 (described in detail hereinafter) having an input 29a connected to the output of the 
35 node 1 5 and an output 29b communicating with a summing input of the node 22; and 

a third processor block 30 (described in detail hereinafter) receiving at its input the control vector u(t) and having 
an output 30b communicating with a summing input of the node 22. 



The physical system of the estimator 10 can be represented by a system (1) of linear differential equations having 
40 variable parameters, of the type: 



dt 

45 



d(X( t) ) =A(fc) .£<t) +B(t) u(t) +Ke-<r<t> -Y<t> ) 



y<t) =c(t) x(t) 



so 

where: 



A(t). the said free evolution matrix, represents the transfer function of the block 27; 
55 B(t). the said input matrix, represents the transfer function of the block 30; 

C(t) represents the transfer function of the measurement block 25 and allows selection, of the output variables Y{t) 
from among the estimated state variables X{t) of the system.; and 
Ke(t) is the transfer function of the block 29 and represents a gain. 
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The dependency of the matrices A(t) and B(t) on time indicates how the behaviour of the system is not constant in 
time and depends on the working conditions (system having variable parameters). 

The autoplot device 1 , thanks to the feedback (signal X{t)) brings the output variables Y(t) close to the references 
R set by the operator. This type of differential proportional feedback does not ensure the absence of operating errors, 

s that is differences between references and outputs, caused by perturbations. 

Environmental disturbances which are not measurable and/or not identifiable which act on the physical system 3 
are of various types, moreover the modelling of the physical system, as performed by the estimator 1 0, is obviously not 
perfect; because of these factors errors are generated (operating errors) in the state variables of the system 3, that is 
to say the real value of the state variables of the system 3 differs from the required value. 

10 The object of the present invention is to provide an autopilot device which can cancel dynamic operating errors in 
a dynamic system having constant or variable parameters. 

The preceding object is achieved by the present invention in that it relates to an autopilot device of the type com- 
prising: regulator means receiving a plurality of reference signals at its input and generating at its output control signals 
for a physical system the dynamic behaviour of which can be described by a plurality of state variables; measurement 

15 means cooperating with the said physical system and operable to select, from among the said state variables, a subset 
of measured state variables; estimator means operable to model the said physical system receiving the said control sig- 
nals at its input and generating at its output a plurality of estimated output variables; comparator means receiving the 
said measured output variables at its input together with the said estimated output variables and generating at its output 
an error signal fed to the estimator means themselves; the said estimator means moreover supplying a feedback signal 

20 to the said regulator means which is utilised, together with the said reference signals, to calculate the said control sig- 
nals, characterised in that there is provided in the said estimator means an extended dynamic system which has as its 
state variables a plurality of estimated state variables of the physical system together with a plurality of additional state 
variables which model respective imaginary commands of unknown value which have the same effect on the physical 
system as the said control signals and which introduce operating errors into the dynamic behaviour of the physical sys- 

25 tern 'rtselt 

The present invention also relates to a method for cancelling operating errors in the dynamic behaviour of a system 
having variable parameters comprising the steps of: supplying a plurality of input reference signals to regulator means 
for generating output control signals; supplying the said control signals to a physical system the dynamic behaviour of 
which can be described by a plurality of state variables; selecting from among the said state variables a subset of meas- 

30 ured output variables; modelling the said physical system by means of a model receiving the said control signals at its 
input and generating at its output a plurality of estimated output variables; comparing the said measured output varia- 
bles with the said estimated output variables to generate an error signal supplied to the said model; and supplying a 
feedback signal to the said regulator means together with the said reference signals for the calculation of the said con- 
trol signals, characterised in that it includes the step of providing in the said model an extended dynamic system which 

35 has as its state variables a plurality of estimated state variables of the physical system together with a plurality of addi- 
tional state variables which model respective imaginary commands of unknown value which have the same effect on 
the physical system as the said control signals and which introduce the said operating errors in the dynamic behaviour 
of the physical system itself. 

The invention will now be illustrated with reference to the attached drawings which represent a preferred but non- 
40 limitative embodiment thereof, in which: 

Figure 1 schematically illustrates an autopilot device of Known type; and 

Figure 2 schematically illustrates an autopilot device formed according to the principles of the present invention. 

45 In Figure 2 there is shown an autopilot device 1a formed according to the principles of the present invention. The 
device 1a acts to control a physical system 40 (shown schematically) receiving at its input a control vector u(t) and gen- 
erating at its output a plurality of state variables X(t) which describe the dynamic behaviour of the system. In particular, 
the state variables X(t) are supplied to a measurement block 42 which selects, from among the various state variables 
X(t) of the physical system 40 a subset of state variables Y(t) which represent the measured output variables. For exarn- 

so pie, the physical system 40 may comprise a vehicle (not shown) in particular a vehicle which moves in a fluid (for exam- 
ple a submarine) which is provided with a plurality of sensors (not shown) acting to measure the instantaneous value 
of the output variables Y(t). The output variables Y(t) are defined in general by an N-dimensional vector Y(t) comprising, 
for example, the following variables: 

55 • Yi depth; 

Y 2 heading; 

• Y 3 pitch angle; 

• Y 4 roll angle; 
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Y 5 velocity; and 
• Y n (...); 

The device la comprises a regulator 44 receiving a plurality of reference signals R (reference set) at its input 
5 together with a plurality of limits L and generating at its output the control vector u(t). 

The reference signals R are set manually by an operator (not shown) of the autopilot device 1a and represent the 
desired values of the state variables of the physical system 40 controlled by the autopilot device 1a. The device 1a fur- 
ther comprises an estimator 46 which operates to model the physical system 40, receive the control vector u(t) at its 
input and generates at its output a vector the nature of which will be clarified hereinbelow, supplied as a feedback signal 
10 by means of a line 48 to the regulator 44. 

The estimator 46 acts to model the dynamic behaviour of the physical system 40 by generating an output vector 
Z(r) the nature of which will be clarified hereinafter. 

The estimator 46 further acts to generate a vector Y(t) comprising the estimated values of the measured output 
variables. of the system. 

is The regulator 44 determines the control vector u(t) as a function of the reference signals R, the limits set by the 
operator and as a function of the vector Z(f). 

The output from the measurement block 42 is fed (with positive sign) to a summing block 50 which also receives 
the vector Y{t) (estimated output variables) inverted in sign; the summing block 50 therefore generates at its output an 
error vector e(t) which represents the difference between the vector Y(t) of the measured output variables and the vec- 
20 tor Y(t) of the estimated output variables. 

The estimator 46 comprises an integrator block 52 which has an input 52a communicating with a summing node 
54 and an output 52b communicating with the input of a measurement block 56 generating at its output the vector Y(t) 
of the estimated output variables. 

The estimator 46 further comprises: 

25 

a first processor block 60 (described in detail hereinafter) having an input 60a connected to the output 52b of the 
integrator block 52 and an output 60b communicating with a summing input of the node 54; 
a second processor block 62 (described in detail hereinafter) having an input 62a connected to the output of the 
node 50 and an output 62b communicating with a summing input of the node 54; and 
30 a third processor block 66 (described in detail hereinafter) receiving the control vector u(t) at its input and having 
an output communicating with a summing input of the node 54. 

According to the present invention, for the purpose of cancelling dynamic operating errors of the system it is sup- 
posed that the operating errors of the state variables are generated by an imaginary additional command r\(X) t of 
35 unknown value, which has the same effect on the system as the control vector u(t) and which introduces these operat- 
ing errors into the dynamic behaviour of the system. 

By estimating the said imaginary commands and inserting them into the law of control (as specified hereinafter) the 
operating errors are cancelled. 

For the purpose of calculating such additional imaginary commands there is defined an extended dynamic system 
40 Z(t) which has as its state variables all the state variables X(t) of the physical system 40 together with the additional 
state variables which model respective imaginary commands T|(t). that is to say: 



45 



Z(t) = 



X(t) 

n(t) 



50 

The overall state variables 2(t) of the disturbances n(t) are then estimated by an estimator described by the follow- 
ing set (2) of differential equations: 
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dZ(t) 
dt 



A(t) B(t) 
0 0 





Bit) 






0 



u(t) +KBm(t) (Y(t) -r(t>) 



70 



;s where: 



Y(t) =[c(t) o}i{t) 



z<t) = 



20 



X(t) 

n(t) 



25 X(f)represent the estimated state variables and 

ii(flrepresent the estimated imaginary commands. 

That is to say, by developing the system the following relationship (3) is obtained: 



30 



35 



d_ 
dt 



X(t) 

n(t) 



A(t) X(t) +B(t)-f)(t) +B(t)u(t) +Kex (Y(t) -Y(t)) 

Ken(t) (r(t)-y(t)j 



40 



r(t)=c(t)-x<t) 



45 



50 



From an analysis of the above system (3) of equations it is noted how the effect (that is to say the term B(t)T|(t)) of 
the disturbances r\(\) of the system is identical to that (that is to say the term B(t)u(t) of the commands u(t). that is to say 
the disturbances are comparable to command variables. 

To obtain the set of equations (2) of the said above type one has that: 



the transfer function of the block 60 is of the type: 



55 



BNSDOCID: <EP 0872785A1 J_> 



5 



EP 0 872 785 A1 



Ait) Bit) 
0 0 



where A(t) is the free evolution matrix of the system and B(t) represents the input matrix of the system. The math- 
10 ematical and physical significance of the free evolution matrix and the input matrix of the system is known and can 
be found, for example, in the text "Theory of Systems" by Sergio Rinaldi, Published by Clup - Milan or in the text 
"Linear Optimal Control Systems" by Huibert Kwakernaak, Raphael Sivan" Published by John Wiley & Sons. 
The transfer function of the block 66 is of the type: 

15 r t 

Bit) 
0 



20 

where B(t) represents the input matrix. 

The transfer function of the measurement block 56 is of the type: 

25 



[c<t> 0] 



where C(t) represents the transfer function which allows the output variables to be selected from among all the esti- 
30 mated state variables in the estimator; and 

the transfer function of the block 62 is Kem(t) which represents a gain matrix: 



35 



Kemit) = 



Kexit) 
Ken(t) 



40 

The estimator 46 determines r^t) in such a way as to reduce to zero the error between estimation and measure- 
ment of the observed state variables 

y<t)-y(t) . 

45 

In fact, the estimation error 

e<t>=r<t)-Y(t) 

50 

multiplied by the gain Kem(t) is integrated by the integrator block 52 and contributes the to the determination of the 
estimation. The inputs from the integrator block 52, which represent the velocities of the state variables, will be nil 
when the estimation error is zero for constant command signals u(t). 

55 

The additional state variables present in the vector Z(t) at the output from the integrator block 52 can be selected 
by means of a selector block 70 having an input 70a communicating with the output 52b and an output 70b at which the 
additional state variables representative of the imaginary commands rj(t) are present. 
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A transfer function of the selector block 70 is conveniently of the type: 

[o rdj 



where Id represents the identity matrix. 

The values of the additional state variables -n(t) corning out from the block 70 can now be displayed and therefore 
to utilised by the operator of the physical system 40 for a possible manual correction of the system 40, as constant com- 
ponents (offset) to apply to command signals to compensate the operating errors. The compensation may also be 
effected in an entirely automatic manner by determining the commands u(t) as a function of the errors of the state var- 
iables and the estimated disturbances T|(t). 

In particular, the regulator 44 receiving the references R at its input generates commands u(t) at its output as: 

15 

u ( t) =Krefer •!*( t) -Kr{ t) X{ t> -fj ( t> 

20 where: Krefer(t) represents the gain of the reference variables: Kr(t) represents a set of gains of the state variables; X{t) 
represents the estimated state variables and r\(t) represents the imaginary commands. 

This regulator 44, together with the estimator 46 described, is able to cancel operating errors on the variables which 
it is desired to regulate thanks to the contribution of the estimated imaginary commands i\(t). The invention therefore 
comprises: the assimilation of the pertabations which act on the system into imaginary commands, the estimation of 

25 the imaginary command rj(f) by means of the above described estimator and the use of the estimated imaginary com- 
mands in the law of control. 

Claims 

30 1 . An autopilot device of the type comprising: 

regulator means (44) receiving a plurality of reference signals (R) at its input and generating at its output con- 
trol signals (u(t)) for a physical system (40) the dynamic behaviour of which can be described by a plurality of 
state variables (X(t)); 

measurement means (42) cooperating with the said physical system (40) and operable to select from the said 
state variables (X(t)) a subset of measured output variables (Y(t)); 

estimator means (46) operable to model the said physical system (40) receiving the said control signals (u(t)) 
at its input and generating at its output a plurality of estimated output variables (Y{t)): 
comparator means (50) receiving measured output variables (Y(t)) together with the said estimated output var- 
iables (Y(t)) at its input and generating at its output an error signal (e(t)) utilised by the estimator means (46); 
the said estimator means (46) further supplying a feedback signal to the said regulator means (44) utilised, 
together with the said reference signals (R), to calculate the said control signals (u(t)), 

characterised in that an extended dynamic system (2(t) is provided in the said estimator means (46), 
which has as state variables a plurality of estimated state variables (X(0) of the physical system (40) together 
with a plurality of additional state variables which model respective imaginary commands (A(0) of unknown 
value which have the same effect on the physical system (40) as the said control signals (u(t)) and which intro- 
duce the operating errors into the dynamic behaviour of the physical system (40). 

2. A device according to Claim 1 , characterised in that the said estimator means are described by the system of dif- 
so ferential equations: 



55 
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dZ(t) 
dfc 



A(t) B(t) 
0 0 





Bit) 






0 



u(t) +Kem (r(t) -r<t) ) 



10 



y<t)=[c<t> o]-z(t) 



where: 



Y(t) is a vector which represents the said measured output variables; 
Y(t) is a vector which represents the said estimated output variables; 

Z(t) is a vector comprising the said estimated state variables (X(f)) together with the said additional state var- 
iables {i\(t)) t 

A(t) is the free evolution matrix of the modelled system; 
B(t) represents the input matrix of the modelled system; 

C(t) represents a transfer function which allows the said estimated output variables (Y(t)) to be selected from 
among the estimated state variables in the said estimator means; and 
Kern represents a gain matrix. 

3- A device according to Claim 1 or Claim 2, characterised in that the said estimator means (46) comprise: 

integration means (52) having an input (52a) communicating with a summing node (54) and an output (52b) 
communicating with the input of selection means (56) generating the said estimated output variables (K(f)) at 
its output; 

first processor means (60) having an input (60a) connected to the output (52b) of the said integration means 
(52) and an output (60b) communicating with a summing input of the said node (54); 

second processor means (62) having an input (62a) receiving the said error signal (e(t)) and an output (62b) 
communicating with a summing input of the said node (54); and 

third processor means (66) receiving the said control signal (u(t)) at its input and having an output (30b) 
communicating with a summing input of the said node (54). 

35 4. A device according to Claim 3, characterised in that the transfer function of the first processor (60) is of the type: 



15 



20 



25 



30 



40 



A(t) Bit) 
0 0 



45 



where A(t) is the free evolution matrix of the modelled system and B(t) is the input matrix of the modelled system; 

the transfer function of the said second processor means (62) includes gain elements (Kern); and 
the transfer function of the said third processor means (66) is of the type: 



50 



B(t) 
0 



55 



where B(t) is the input matrix of the modelled system. 

5. A device according to any preceding claim, characterised in that the said physical system comprises a vehicle 
which moves in a fluid, particularly a submarine. 
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6. A device according to any preceding claim, characterised in that the said regulator means (44) generate the said 
output control signals (u(t)) as: 

u(t) =Krefer -H'(t) -Kr(t)X(t) -q (t) 



where: 



io u(t) are the said control signals; 

R(t) are the said reference signals; 

T)(f) represents the said imaginary commands; 

X{t) are the said estimated state variables; 

Krefert(t) represents a gain of the reference variables; and 
15 Kr(t) represents a set of gains of the state variables. 

7. A method for cancelling operating errors in the dynamic behaviour of a system having variable parameters com- 
prising the steps of: 

20 - feeding a plurality of input reference signals (R) to regulator means (44) to generate control signals (u(t)) at its 
output; 

- feeding the said control signals (u(t)) to a physical system (40) the dynamic behaviour of which can be 
described by a plurality of state variables (X(t)); 

- selecting (42) from the said state variables (X(t)) a subset of measured output variables (Y(t)); 

25 - modelling the said physical system (40) by means of a model receiving the said control signals (u(t)) at its input 
and generating a plurality of estimated output variables ( Y(t))\ 

- comparing (50) the said measured output variables (Y(t)) with the said estimated output variables ( Y(t)) to gen- 
erate an error signal (e(t)) utilised by the said model (46); and 

- supplying a feedback signal to the said regulator means (44) together with the said reference signals (R) for 
30 calculating the said control signals (u(t)), 

characterised in that it includes the steps of providing in the said model an extended dynamic system 
(Z(t)) which has as state variables a plurality of estimated state variables (X{t)) of the physical system (40) 
together with a plurality of additional state variables which model respective imaginary commands (fj(f)) of 
unknown value which have the same effect on the physical system (40) as the said control signals (u(t)) and 
35 which introduce the said operating errors into the dynamic behaviour of the physical system (20) itself. 

8. A method according to Claim 7, characterised in that the said model is descrfoed by a system of differential equa- 
tions of the type: 



40 



dZ{t) , 
dt 



A(t) 


Bit) 




Bit) 








0 


0 


0 



u(t) +Xem *(y<t> -Y(t>> 



45 



Y(t)=[C(t) 0]-Z(t) 



where: 

so 

Y(t) is a vector which represents the said measured output variables; 
Y(t) is a vector which represents the said estimated output variables; 

2(t) is a vector comprising the said estimated state variables (X(f)) together with the said additional state var- 
iables (n(0): 

55 A(t) is the free evolution matrix of the modelled system; 

B(t) represents the input matrix of the modelled system; 

C(t) represents a transfer function which allows the said estimated state variables (Y(t)) to be selected from 
among the estimated state variables in the said estimator means; and 
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Kem represents a gain matrix. 

9. A method according to Claim 7 or Claim 8, characterised in thai the said step of supplying at the input a plurality of 
reference signals (R) to regulator means (44) to generate output control signals (u(t)) comprises the step of gener- 
5 ating at the output the said control signals (u(t)) according to the expression: 



u(t)=Krefeir vR(t) -Kr(t)X<t) -f\(t) 

10 

where: 

u(t) are the said control signals; 
R(t) are the said reference signals; 
75 i\{t) represents the said imaginary commands; 

X(f) are the said estimated state variables; 
Krefert(t) represents a gain of the reference variables; and 
Kr(t) represents a set of gains of the state variables. 



25 



30 



35 



40 



45 



50 
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